#ifndef __CONTROL_H
#define __CONTROL_H

#include "pid.h"

#define GRAVITY_MSS	9.8f						 //Gravitational acceleration,unit:m/s^2
#define MECHANICAL_MEDIAN_ANGLE -1.12f  //Mechanical median angle
#define L 0.05f											 //The distance from the center of the wheel to the center of mass
#define WEIGHT	0.857f  								 //Centroid weight,unit:kg
#define J WEIGHT*L*L								 //Moment of inertia,unit: kg*m^2
#define PI 3.141f		
#define WHEEL_DIAMETER 0.11f				 //WHEEL DIAMETER,unit:m
#define GEAR_RATIO 36.f							 //Motor gearbox gear ratio

typedef struct
{
	float mileage; //unit:m
	float speed;	 //unit:m/s
	float w_rotate;
	
	float motor_speed_ref;
}control_info_t;

void motor_speed_control( float ref_right,float ref_left );
float attitude_pid(float ref_angle);
float control_resolution(float u);
void attitude_control(float dT);
void update_car_mileage(float right_mileage,float left_mileage);
float mileage_control(float mileage_ref);
#endif


